#!/usr/bin/env python3

# 通用的工具函数

import rospy
import time
import numpy as np
from std_msgs.msg import String
from std_msgs.msg import Int32
from mavproxy_ros1.msg import FlightCmd



class MavproxyTool():
    def __init__(self):
        self.sent_seq = 0
        pass

    def get_flight_cmd(self, command_full):
        # cmd type
        command_type = command_full.split(" ")[0]
        rospy.loginfo(command_type)

        # cmd param
        param_list = [float(x) for x in command_full.split(" ")[1:] ]
        rospy.loginfo(param_list)

        # 根据命令类型，次序填入参数，封装到消息中
        current_cmd = FlightCmd()
        current_cmd.header.seq = self.sent_seq
        current_cmd.header.stamp = rospy.Time.now()
        current_cmd.header.frame_id = "local_origin"
        current_cmd.cmdtype = command_type
        current_cmd.paramdata = param_list
        self.sent_seq += 1

        return current_cmd

    def get_input_cmd(self, command_string):
        input_cmd = String()
        input_cmd.data = command_string

        return input_cmd

    def get_quaternion_to_euler(self, q0, q1, q2, q3):
        yaw = pitch = roll = 0

        t3 = +2.0 * (q0 * q3 + q1 * q2)
        t4 = +1.0 - 2.0 * (q1 * q1 + q3 * q3)
        yaw = np.arctan2(t3, t4)

        t2 = +2.0 * (q0 * q2 - q3 * q1)
        t2 = +1.0 - 2.0 * (q2 * q2 + q3 * q3)
        pitch = np.arctan2(t2, q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3)

        t0 = +2.0 * (q0 * q1 + q2 * q3)
        t1 = +1.0 - 2.0 * (q1 * q1 + q2 * q2)
        roll = np.arctan2(t0, t1)

        return (yaw, pitch, roll)

    def get_euler_to_quaternion(self, yaw, pitch, roll):
        q0 = q1 = q2 = q3 = 0

        return (q0, q1, q2, q3)
